#include "ODESimulator.h"
#include "ODECommon.h"
#include "ODECustomMesh.h"
#include <list>
#include <geometry/ConvexHull2D.h>
#include <utils/EquivalenceMap.h>
#include <ode/ode.h>
#include <Timer.h>

//mostly for robot constraints
const static double kErrorReductionParameter = 0.95;
const static double kDampedLeastSquaresParameter = 1e-6;

//merging contacts for faster simulation?
const static bool kMergeContacts = true;
const static double kMergePosTolerance = 1e-3;
const static double kMergeOriTolerance = 1e-2;

//environment simulation properties
const static double kEnvCoeffRestitution = 0.1;
const static bool kSoftEnvContact = false;
const static double kEnvCoeffStiffness = 80000;
const static double kEnvCoeffDamping = 20000;

/*
//for some reason these work better in ode 0.9
const static double kErrorReductionParameter = 0.2;
const static double kDampedLeastSquaresParameter = 1e-5;
const static double kEnvCoeffRestitution = 0.0;
const static bool kMergeContacts = false;
const static double kMergePosTolerance = 3e-3;
const static double kMergeOriTolerance = 1e-1;
const static bool kSoftEnvContact = true;
const static double kSoftEnvContactERP = 0.2;
const static double kSoftEnvContactCFM = 1e-4;
*/

inline Real ERPFromSpring(Real timestep,Real kP,Real kD)
{
  return timestep*kP/(timestep*kP+kD);
}

inline Real CFMFromSpring(Real timestep,Real kP,Real kD)
{
  return 1.0/(timestep*kP+kD);
}

ODESurfaceProperties GetSurfaceProperties(const Environment* env,int tri=-1)
{
  ODESurfaceProperties res;
  res.kRestitution = kEnvCoeffRestitution;
  res.kFriction = (tri<0?env->kFriction[0]:env->kFriction[tri]);
  if(kSoftEnvContact) {
    res.kStiffness = kEnvCoeffStiffness;
    res.kDamping = kEnvCoeffDamping;
  }
  else {
    res.kStiffness = res.kDamping = Inf;
  }
  return res;
}



struct ODEObject 
{
  ODEObject() {
    numRefs = 0;
    initialized = false;
  }
  ~ODEObject() { 
    if(initialized) {
      //printf("Closing ODE...\n"); 
      dCloseODE();
    }
  }

  void ref()
  {
    if(numRefs == 0) {
      //printf("Initializing ODE...\n");
      dInitODE();
      InitODECustomMesh();
      initialized = true;
    }
    numRefs++;
  }

  void deref()
  {
    numRefs--;
  }

  int numRefs;
  bool initialized;
};

ODEObject g_ODE_object;



//stuff for contact detection callbacks
struct ODEContactResult
{
  dGeomID o1,o2;
  vector<dContactGeom> contacts;
  vector<dJointFeedback> feedback;
};

const static int max_contacts = 1000;
static dContactGeom gContactTemp[max_contacts];
static list<ODEContactResult> gContacts;



ODESimulator::ODESimulator()
{
  worldID = dWorldCreate();
  contactGroupID = dJointGroupCreate(0);
  //envSpaceID = dHashSpaceCreate(0);
  //dHashSpaceSetLevels(envSpaceID,-2,1);
  envSpaceID = dSimpleSpaceCreate(0);

  dWorldSetERP(worldID,kErrorReductionParameter);
  dWorldSetCFM(worldID,kDampedLeastSquaresParameter);

  g_ODE_object.ref();
}

void ODESimulator::SetGravity(const Vector3& g)
{
  dWorldSetGravity(worldID,g.x,g.y,g.z);
}

void ODESimulator::SetERP(double erp)
{
  dWorldSetERP(worldID,erp);
}

void ODESimulator::SetCFM(double cfm)
{
  dWorldSetERP(worldID,cfm);
}

ODESimulator::~ODESimulator()
{
  dJointGroupDestroy(contactGroupID);
  for(size_t i=0;i<envMeshes.size();i++)
    delete envMeshes[i];
  for(size_t i=0;i<robots.size();i++)
    delete robots[i];
  dSpaceDestroy(envSpaceID);
  dWorldDestroy(worldID);

  g_ODE_object.deref();
}

//dGeomID planeGeomID = 0;

void ODESimulator::AddEnvironment(const Environment& env)
{
  envs.push_back(&env);
  envMeshes.resize(envMeshes.size()+1);
  envMeshes.back() = new ODETriMesh;
  envMeshes.back()->Create(env.mesh,envSpaceID);
  Matrix4 I; I.setIdentity();
  envMeshes.back()->SetLastTransform(I);
  envMeshes.back()->surf() = GetSurfaceProperties(&env);
  //the index of the environment is encoded as -1-index
  dGeomSetData(envMeshes.back()->geom(),(void*)(-(int)envs.size()));
  dGeomSetCategoryBits(envMeshes.back()->geom(),0x1);
  dGeomSetCollideBits(envMeshes.back()->geom(),0xffffffff ^ 0x1);
  /*
  planeGeomID = dCreatePlane(envSpaceID,0,0,1,0);
  dGeomSetData(planeGeomID,(void*)-1);
  */
}

void ODESimulator::AddRobot(Robot& robot)
{
  robots.push_back(new ODERobot(robot));
  robots.back()->Create(worldID);
  //robotStances.resize(robots.size());
  for(size_t i=0;i<robot.links.size();i++)
    if(robots.back()->triMesh(i) && robots.back()->geom(i)) {
      if(robots.back()->robot.parents[i] == -1) { //treat as part of the terrain
	dGeomSetCategoryBits(robots.back()->geom(i),0x1);
	dGeomSetCollideBits(robots.back()->geom(i),0xffffffff ^ 0x1);
      }
      else {
	dGeomSetCategoryBits(robots.back()->geom(i),0x4);
	dGeomSetCollideBits(robots.back()->geom(i),0xffffffff);
      }
    }
}

void ODESimulator::AddObject(const RigidObject& object)
{
  objects.push_back(new ODERigidObject(object));
  objects.back()->Create(worldID,envSpaceID);
  dGeomSetData(objects.back()->geom(),(void*)(objects.size()-1));
  dGeomSetCategoryBits(objects.back()->geom(),0x2);
  dGeomSetCollideBits(objects.back()->geom(),0xffffffff);
}


void ODESimulator::Step(Real dt)
{
  //Timer timer;
  //double collisionTime,stepTime,updateTime;

  timestep=dt;
  DetectCollisions();

  //collisionTime = timer.ElapsedTime();
  //timer.Reset();

  //weird thing for triangle meshes
  for(size_t i=0;i<robots.size();i++) {
    robots[i]->UpdateLastTransforms();
  }
  for(size_t i=0;i<objects.size();i++) {  
    objects[i]->UpdateLastTransform();
  }

  StepDynamics(dt);

  //stepTime = timer.ElapsedTime();
  //timer.Reset();

  for(map<pair<ODEObjectID,ODEObjectID>,ODEContactList>::iterator i=contactList.begin();i!=contactList.end();i++) {  
    ODEContactList& cl=i->second;
    cl.forces.clear();
    for(size_t j=0;j<cl.feedbackIndices.size();j++) {
      int k=cl.feedbackIndices[j];
      Assert(k >= 0 && k < (int)gContacts.size());
      list<ODEContactResult>::iterator cres=gContacts.begin();
      advance(cres,k);
      Vector3 temp;
      for(size_t i=0;i<cres->feedback.size();i++) {
	CopyVector(temp,cres->feedback[i].f1);
	cl.forces.push_back(temp);
      }
    }
  }
  //updateTime = timer.ElapsedTime();

  //printf("ODE simulation step: %g collision, %g step, %g update\n",collisionTime,stepTime,updateTime);
}


struct EqualPlane
{
  //position tolerance and angle tolerance, in radians
  EqualPlane(double xtol,double atol) : ptol(xtol),catol(Cos(atol)) {}

  //assumes normalized normals
  bool operator () (const dContactGeom& a,const dContactGeom& b)
  {
    Vector3 an,bn;
    CopyVector(an,a.normal);
    CopyVector(bn,b.normal);
    Real ndot = an.dot(bn);
    if(ndot > catol) {
      Vector3 ax,bx;
      CopyVector(ax,a.pos);
      CopyVector(bx,b.pos);
      return  FuzzyEquals(ax.dot(an),bx.dot(an),ptol) && 
      	FuzzyEquals(ax.dot(bn),bx.dot(bn),ptol);
    }
    return false;
  }

  double ptol,catol;
};

struct EqualPoint
{
  //position tolerance and angle tolerance, in radians
  EqualPoint(double xtol) : ptol(xtol) {}

  //assumes normalized normals
  bool operator () (const dContactGeom& a,const dContactGeom& b)
  {
    Vector3 ax,bx;
    CopyVector(ax,a.pos);
    CopyVector(bx,b.pos);
    return  ax.distanceSquared(bx) < Sqr(ptol);
  }

  double ptol;
};


/** Two ways of merging frictionless contacts without resorting to 6D
 * wrench space
 * 1) interior points in 2D convex hull, with the projection defined 
 *    by the contact normal
 * 2) interior points in convex cone at a single contact point (TODO)
 * 
 * If contacts have friction, the convex hull method only works when the
 * points are on the same plane.
 */
void CHContactsPlane(vector<dContactGeom>& contacts)
{
  if(contacts.empty()) return;
  if(contacts.size() <= 2) return;

  Vector3 c(Zero),n(Zero);
  for(size_t i=0;i<contacts.size();i++) {
    Vector3 cx,nx;
    CopyVector(cx,contacts[i].pos);
    CopyVector(nx,contacts[i].normal);
    c += cx;
    n += nx;
  }
  c /= contacts.size();
  n.inplaceNormalize();

  //get the deepest contact
  size_t deepest = 0;
  for(size_t i=1;i<contacts.size();i++) 
    if(contacts[i].depth > contacts[deepest].depth) deepest=i;

  //make a plane
  Vector3 x,y;
  n.getOrthogonalBasis(x,y);
  //Real nofs = n.dot(c);
  Real xofs = x.dot(c);
  Real yofs = y.dot(c);
  vector<Vector2> pt2d(contacts.size());
  for(size_t i=0;i<contacts.size();i++) {
    Vector3 cx;
    CopyVector(cx,contacts[i].pos);
    pt2d[i].x = x.dot(cx)-xofs;
    pt2d[i].y = y.dot(cx)-yofs;
  }

  //get the convex hull
  vector<Vector2> ch(contacts.size());
  vector<int> mapping(contacts.size());
  int num=Geometry::ConvexHull2D_Chain_Unsorted(&pt2d[0],contacts.size(),&ch[0],&mapping[0]);
  vector<dContactGeom> temp(num);
  bool addDeepest = true;
  for(int i=0;i<num;i++) {
    Assert(mapping[i] < (int)contacts.size());
    temp[i] = contacts[mapping[i]];
    if(mapping[i] == (int)deepest)
      addDeepest = false;
  }
  if(addDeepest)
    temp.push_back(contacts[deepest]);
  swap(temp,contacts);
}

void interpolate(const dContactGeom& a,const dContactGeom& b,Real u,dContactGeom& x)
{
  for(int i=0;i<3;i++) {
    x.pos[i] = (1-u)*a.pos[i]+u*b.pos[i];
  }
  Vector3 an,bn;
  CopyVector(an,a.normal);
  CopyVector(bn,b.normal);
  Vector3 n = (1-u)*an+u*bn;
  n.inplaceNormalize();
  CopyVector(x.normal,n);

  x.depth = Max(a.depth,b.depth);
  x.g1 = a.g1;
  x.g2 = a.g2;
  Assert(x.g1 == b.g1);
  Assert(x.g2 == b.g2);
}

void MergeContacts(vector<dContactGeom>& contacts,double posTolerance,double oriTolerance)
{
  EqualPlane eq(posTolerance,oriTolerance);
  vector<vector<int> > sets;
  EquivalenceMap(contacts,sets,eq);

  vector<dContactGeom> res;
  vector<dContactGeom> temp;
  for(size_t i=0;i<sets.size();i++) {
    if(sets[i].empty()) continue;
    temp.resize(sets[i].size());
    for(size_t j=0;j<sets[i].size();j++)
      temp[j] = contacts[sets[i][j]];
    CHContactsPlane(temp);

    EqualPoint eq(posTolerance);
    vector<vector<int> > sets;
    EquivalenceMap(temp,sets,eq);

    res.reserve(res.size()+sets.size());
    for(size_t k=0;k<sets.size();k++) {
      //find centroids
      dContactGeom mean;
      for(size_t m=0;m<sets[k].size();m++) {
	if(m == 0) 
	  mean = temp[sets[k][m]];
	else {
	  dContactGeom ctemp=mean;
	  interpolate(ctemp,temp[sets[k][m]],1.0/(1.0+Real(m)),mean);
	}
      }
      res.push_back(mean);
    }
  }
  swap(contacts,res);
}

void collisionCallback(void *data, dGeomID o1, dGeomID o2)
{
  //ODESimulator* sim = reinterpret_cast<ODESimulator*>(data);
  Assert(!dGeomIsSpace(o1) && !dGeomIsSpace(o2));
  
  int num = dCollide (o1,o2,max_contacts,gContactTemp,sizeof(dContactGeom));
  vector<dContactGeom> vcontact(num);
  int numOk = 0;
  for(int i=0;i<num;i++) {
    if(gContactTemp[i].g1 == o2 && gContactTemp[i].g2 == o1) {
      printf("Swapping contact\n");
      std::swap(gContactTemp[i].g1,gContactTemp[i].g2);
      for(int k=0;k<3;k++) gContactTemp[i].normal[k]*=-1.0;
      std::swap(gContactTemp[i].side1,gContactTemp[i].side2);
    }
    Assert(gContactTemp[i].g1 == o1);
    Assert(gContactTemp[i].g2 == o2);
    vcontact[numOk] = gContactTemp[i];
    const dReal* n=vcontact[numOk].normal;
    if(Sqr(n[0])+Sqr(n[1])+Sqr(n[2]) < 0.9) {
      printf("Warning, degenerate contact with normal %f %f %f\n",vcontact[numOk].normal[0],vcontact[numOk].normal[1],vcontact[numOk].normal[2]);
      //continue;
    }
    numOk++;
  }
  vcontact.resize(numOk);
  if(kMergeContacts) {
    MergeContacts(vcontact,kMergePosTolerance,kMergeOriTolerance);
  }
  if(vcontact.size() > 0) {
    /*
    if(numOk != (int)vcontact.size())
      cout<<numOk<<" contacts between env "<<(intptr_t)dGeomGetData(o2)<<" and body "<<(intptr_t)dGeomGetData(o1)<<"  (clustered to "<<vcontact.size()<<")"<<endl;
    */
    gContacts.push_back(ODEContactResult());
    gContacts.back().o1 = o1;
    gContacts.back().o2 = o2;
    swap(gContacts.back().contacts,vcontact);
  }
}

void ODESimulator::ClearCollisions()
{
  dJointGroupEmpty(contactGroupID);
  contactList.clear();
}

void ODESimulator::GetSurfaceParameters(const ODEObjectID& a,const ODEObjectID& b,dSurfaceParameters& surface) const
{
  //TODO: base the friction on the properties of the contact point
  //completely rigid contact
  surface.mode = dContactApprox1;
  //surface.mode = 0;
  surface.bounce = 0;
  surface.bounce_vel = 0;
  
  //printf("GetSurfaceParameters a = %d,%d, b = %d,%d\n",a.type,a.index,b.type,b.index);
  ODETriMesh *ma,*mb;
  if(a.type == 0) {
    Assert(a.index < (int)envs.size());
    ma = envMeshes[a.index];
  }
  else if(a.type == 1) 
    ma = robots[a.index]->triMesh(a.bodyIndex);
  else if(a.type == 2)
    ma = objects[a.index]->triMesh();
  else Abort();
  if(b.type == 0) {
    Assert(b.index < (int)envs.size());
    mb=envMeshes[b.index];
  }
  else if(b.type == 1) 
    mb=robots[b.index]->triMesh(b.bodyIndex);
  else if(b.type == 2) 
    mb=objects[b.index]->triMesh();
  else Abort();

  const ODESurfaceProperties &propa=ma->surf(),&propb=mb->surf();
  if(!IsInf(propa.kStiffness) || !IsInf(propb.kStiffness)) {
    surface.mode |= dContactSoftERP | dContactSoftCFM;
    Real kStiffness = 1.0/(1.0/propa.kStiffness+1.0/propb.kStiffness);
    Real kDamping = 1.0/(1.0/propa.kDamping+1.0/propb.kDamping);
    surface.soft_erp = ERPFromSpring(timestep,kStiffness,kDamping);
    surface.soft_cfm = CFMFromSpring(timestep,kStiffness,kDamping);
    //printf("Joint stiffness %g, damping %g\n",kStiffness,kDamping);
    //printf("ERP = %g, CFM = %g\n",surface.soft_erp,surface.soft_cfm);
  }
  surface.mu = 2.0/(1.0/propa.kFriction+1.0/propb.kFriction);
  surface.bounce = 0.5*(propa.kRestitution+propb.kRestitution);
  if(surface.bounce != 0)
    surface.mode |= dContactBounce;
}

void ODESimulator::SetupContactResponse(const ODEObjectID& a,const ODEObjectID& b,int feedbackIndex,ODEContactResult& c)
{
  dContact contact;
  GetSurfaceParameters(a,b,contact.surface);
  dBodyID b1 = dGeomGetBody(c.o1);
  dBodyID b2 = dGeomGetBody(c.o2);
  c.feedback.resize(c.contacts.size());
  for(size_t k=0;k<c.contacts.size();k++) {
    //add contact joint to joint group
    contact.geom = c.contacts[k];
    Assert(contact.geom.g1 == c.o1 || contact.geom.g1 == c.o2);
    Assert(contact.geom.g2 == c.o1 || contact.geom.g2 == c.o2);
    Assert(contact.geom.depth >= 0);
    
    Assert(contact.geom.g1 == c.o1);
    dJointID joint = dJointCreateContact(worldID,contactGroupID,&contact);
    dJointSetFeedback(joint,&c.feedback[k]);
    //if(b2==0)
    //dJointAttach(joint,b2,b1);
      //else
      dJointAttach(joint,b1,b2);
  }
  //if contact feedback is enabled, do it!
  pair<ODEObjectID,ODEObjectID> cindex;
  bool reverse = false;
  if(b < a) {
    cindex.first = b;
    cindex.second = a;
    reverse = true;
  }
  else {
    cindex.first = a;
    cindex.second = b;
  }
  ODEContactList* cl=NULL;
  if(contactList.count(cindex) != 0) {
    cl=&contactList[cindex];
  }
  else {
    //check if there's a generic robot feedback list
    bool checkRobot=false;
    if(cindex.first.type == 1 && cindex.first.bodyIndex != -1) {
      cindex.first.bodyIndex=-1;
      checkRobot=true;
    }
    if(cindex.second.type == 1 && cindex.second.bodyIndex != -1) {
      cindex.second.bodyIndex=-1;
      checkRobot=true;
    }
    if(checkRobot) {
      if(contactList.count(cindex) != 0) 
	cl=&contactList[cindex];
    }
  }
  if(cl) {
    size_t start=cl->points.size();
    cl->points.resize(start+c.contacts.size());
    for(size_t k=0;k<c.contacts.size();k++) {
      Assert(k+start < cl->points.size());
      CopyVector(cl->points[k+start].x,c.contacts[k].pos);
      CopyVector(cl->points[k+start].n,c.contacts[k].normal);
      cl->points[k+start].kFriction = contact.surface.mu;
      if(reverse)
	cl->points[k+start].n.inplaceNegative();
    }
    Assert(feedbackIndex >= 0 && feedbackIndex < (int)gContacts.size());
    cl->feedbackIndices.push_back(feedbackIndex);
  }
}

void ODESimulator::DetectCollisions()
{
  dJointGroupEmpty(contactGroupID);
  //clear feedback structure
  for(map<pair<ODEObjectID,ODEObjectID>,ODEContactList>::iterator i=contactList.begin();i!=contactList.end();i++) {
    i->second.points.clear();
    i->second.forces.clear();
    i->second.feedbackIndices.clear();
  }
  gContacts.clear();

  pair<ODEObjectID,ODEObjectID> cindex;
  //call the collision routine between objects and the world
  dSpaceCollide(envSpaceID,(void*)this,collisionCallback);
  int jcount=0;
  for(list<ODEContactResult>::iterator j=gContacts.begin();j!=gContacts.end();j++,jcount++) {
		//// int is not necessarily big enough, use intptr_t
		//int o1 = (int)dGeomGetData(j->o1);
		//int o2 = (int)dGeomGetData(j->o2);
		intptr_t o1 = (intptr_t)dGeomGetData(j->o1);
		intptr_t o2 = (intptr_t)dGeomGetData(j->o2);
    if(o1 < 0) {  //it's an environment
      cindex.first = ODEObjectID(0,(-o1-1));
    }
    else {
      cindex.first = ODEObjectID(2,o1);
    }
    if(o2 < 0) {  //it's an environment
      cindex.second = ODEObjectID(0,(-o2-1));
    }
    else {
      cindex.second = ODEObjectID(2,o2);
    }
    if(o1 < 0 && o2 < 0) {
      fprintf(stderr,"Warning, detecting terrain-terrain collisions?\n");
    }
    else {
      j->feedback.resize(j->contacts.size());
      SetupContactResponse(cindex.first,cindex.second,jcount,*j);
    }
  }

  for(size_t i=0;i<robots.size();i++) {
    cindex.first = ODEObjectID(1,i);
    list<ODEContactResult>::iterator gContactStart = --gContacts.end();

    //call the collision routine between the robot and the world
    dSpaceCollide2((dxGeom *)robots[i]->space(),(dxGeom *)envSpaceID,(void*)this,collisionCallback);
    ++gContactStart;
    //setup the contact "joints" and contactLists
    for(list<ODEContactResult>::iterator j=gContactStart;j!=gContacts.end();j++,jcount++) {
      bool isBodyo1 = false, isBodyo2 = false;
      for(size_t k=0;k<robots[i]->robot.links.size();k++) {
	if(robots[i]->triMesh(k) && j->o1 == robots[i]->geom(k)) isBodyo1=true;
	if(robots[i]->triMesh(k) && j->o2 == robots[i]->geom(k)) isBodyo2=true;
      }
      intptr_t body,obj;
      if(isBodyo2) {
	printf("Warning, ODE collision result lists bodies in reverse order\n");
	Assert(!isBodyo1);
	body = (intptr_t)dGeomGetData(j->o2);
	obj = (intptr_t)dGeomGetData(j->o1);
      }
      else {
	Assert(!isBodyo2);
	Assert(isBodyo1);
	body = (intptr_t)dGeomGetData(j->o1);
	obj = (intptr_t)dGeomGetData(j->o2);
      }
      //printf("Collision between body %d and obj %d\n",body,obj);
      Assert(body >= 0 && body < (int)robots[i]->robot.links.size());
      Assert(obj >= -(int)envs.size() && obj < (int)objects.size());
      if(robots[i]->robot.parents[body] == -1 && obj < 0) { //fixed links
	fprintf(stderr,"Warning, colliding a fixed link and the terrain\n");
	continue;
      }
      cindex.first.bodyIndex = body;
      if(obj < 0) {  //it's an environment
	cindex.second = ODEObjectID(0,(-obj-1));
      }
      else {
	cindex.second = ODEObjectID(2,obj);
      }
      SetupContactResponse(cindex.first,cindex.second,jcount,*j);
    }

    /* TODO:
    //call the self collision routine for the robot
    dSpaceCollide((dxGeom *)robots[i]->space(),(void*)this,collisionCallback);
    */
  }
}

void ODESimulator::EnableContactFeedback(const ODEObjectID& a,const ODEObjectID& b)
{
  pair<ODEObjectID,ODEObjectID> index;
  if(a < b) {
    index.first=a;
    index.second=b;
  }
  else {
    index.first=b;
    index.second=a;
  }
  contactList[index] = ODEContactList();
}

ODEContactList* ODESimulator::GetContactFeedback(const ODEObjectID& a,const ODEObjectID& b)
{
  pair<ODEObjectID,ODEObjectID> index;
  if(a < b) {
    index.first=a;
    index.second=b;
  }
  else {
    index.first=b;
    index.second=a;
  }
  if(contactList.count(index) != 0) {
    return &contactList[index];
  }
  return NULL;
}

bool HasContact(dBodyID a)
{
  if(a == 0) return false;
  int n = dBodyGetNumJoints (a);
  for(int i=0;i<n;i++) {
    dJointID j=dBodyGetJoint (a,i);
    if(dJointGetType(j)==dJointTypeContact) return true;
  }
  return false;
}

bool HasContact(dBodyID a,dBodyID b)
{
  if(a == 0 && b == 0) return false; //two environments
  if(a == 0) Swap(a,b);
  int n = dBodyGetNumJoints (a);
  for(int i=0;i<n;i++) {
    dJointID j=dBodyGetJoint (a,i);
    if(dJointGetType(j)==dJointTypeContact) {
      dBodyID j0=dJointGetBody(j,0);
      dBodyID j1=dJointGetBody(j,1);
      if(j0 == b || j1 == b) return true;
    }
  }
  return false;
}

bool ODESimulator::InContact(const ODEObjectID& a) const
{
  if(a.type == 0) { //environment
    //must loop through all objects to see what is in contact with the
    //environment
    for(size_t i=0;i<objects.size();i++) {
      if(HasContact(objects[i]->body(),0)) return true;
    }
    for(size_t i=0;i<robots.size();i++) {
      for(size_t j=0;j<robots[i]->robot.links.size();j++) {
	if(HasContact(robots[i]->body(j),0)) return true;
      }
    }    
    return false;
  }
  else if(a.type == 2) {//object
    return HasContact(objects[a.index]->body());
  }
  else {
    if(a.bodyIndex >= 0)
      return HasContact(robots[a.index]->body(a.bodyIndex));
    else { //any robot link
      for(size_t i=0;i<robots[a.index]->robot.links.size();i++)
	if(HasContact(robots[a.index]->body(i))) return true;
      return false;
    }
  }
  return false;
}

bool ODESimulator::InContact(const ODEObjectID& a,const ODEObjectID& b) const
{
  vector<dBodyID> bodya, bodyb;
  if(a.type == 0) {
    bodya.push_back(0);
  }
  else if(a.type == 2) {
    bodya.push_back(objects[a.index]->body());
  }
  else {
    if(a.bodyIndex >= 0)
      bodya.push_back(robots[a.index]->body(a.bodyIndex));
    else {
      for(size_t i=0;i<robots[a.index]->robot.links.size();i++)
	if(robots[a.index]->body(i))
	  bodya.push_back(robots[a.index]->body(i));
    }
  }
  if(b.type == 0) {
    bodyb.push_back(0);
  }
  else if(b.type == 2) {
    bodyb.push_back(objects[b.index]->body());
  }
  else {
    if(b.bodyIndex >= 0)
      bodyb.push_back(robots[b.index]->body(b.bodyIndex));
    else {
      for(size_t i=0;i<robots[b.index]->robot.links.size();i++)
	if(robots[b.index]->body(i))
	  bodyb.push_back(robots[b.index]->body(i));
    }
  }
  for(size_t i=0;i<bodya.size();i++)
    for(size_t j=0;j<bodyb.size();j++)
      if(HasContact(bodya[i],bodyb[j])) return true;
  return false;
}

void ODESimulator::StepDynamics(Real dt)
{
  dWorldStep(worldID,dt);
  //dWorldQuickStep(worldID,dt);
}


bool ODESimulator::ReadState(File& f)
{
  for(size_t i=0;i<robots.size();i++) {
    if(!robots[i]->ReadState(f)) {
      fprintf(stderr,"ODESimulator::ReadState(): failed to read robot %d\n",i);
      return false;
    }
  }
  for(size_t i=0;i<objects.size();i++) {
    if(!objects[i]->ReadState(f)) {
      fprintf(stderr,"ODESimulator::ReadState(): failed to read object %d\n",i);
      return false;
    }
  }
  return true;
}

bool ODESimulator::WriteState(File& f) const
{
  for(size_t i=0;i<robots.size();i++) 
    if(!robots[i]->WriteState(f)) return false;
  for(size_t i=0;i<objects.size();i++) 
    if(!objects[i]->WriteState(f)) return false;
  return true;
}
